#include <QCoreApplication>
#include <sched.h>
#include <stdio.h>
#include <sys/mman.h>
#include <native/task.h>
#include <rtdk.h>

#include "trajectoryTest.hpp"
#include "solverTest.hpp"

using namespace KDL;

void upPriority(int priority)
{
    struct sched_param param;
    param.__sched_priority = priority;
    if(sched_setscheduler(0,SCHED_FIFO,&param) == -1)
    {
        perror("sched_setscheduler failed");
    }

    if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1)
    {
        perror("mlockall failed");
    }
}

int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);

    upPriority(50);
    struct sched_param param;
    param.__sched_priority = 99;
    sched_setscheduler(0,SCHED_FIFO,&param);
    int policy, priority;
    pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);

    remove("TestTime.txt") ;

    SolverTest* solverTest = new SolverTest();
    //remove("solverTestTime.txt") ;
    solverTest->chainFkSolverPosTest();
    solverTest->chainIkSolverPosTest_LMA();
    solverTest->chainIkSolverPosTest_NR();
    solverTest->chainIkSolverPosTest_NR_JL();
    solverTest->chainFkVelAndIkVelTest();
    solverTest->chainIdSolverTest_RNE();
    delete solverTest;
    solverTest = NULL;

    TrajectoryTest* trajectoryTest = new TrajectoryTest();
    //remove("trajectoryTestTime.txt") ;
    trajectoryTest->pathLineTest();
    trajectoryTest->pathCircleTest();
    trajectoryTest->pathRoundedCompositeTest();
    delete trajectoryTest;
    trajectoryTest = NULL;

    std::cout << "over..." << std::endl;

    return a.exec();
}
